#include "Planner.h"
#include <vector>
#include "Prm.h"
#include <CGAL/minkowski_sum_2.h>
#include <CGAL/point_generators_2.h>
#include <boost/make_shared.hpp>
#include <CGAL/Boolean_set_operations_2.h>
#include "AStar.h"

using namespace std;

Planner::Planner(Scene* scene) : m_scene(scene)
{
	m_number_of_nodes = 160;

	extract_scenario_from_gui();

	m_collision_detector = new CollisionDetector(m_robot_polygons, &m_obstacles);
	m_multi_robots_sampler = new MultiRobotsSampler(m_robot_polygons[0], m_robot_polygons[1], m_room, m_collision_detector);
	robot_1_sampler = new SingleRobotSampler(1, m_robot_polygons[0], m_room, m_collision_detector);
	robot_2_sampler = new SingleRobotSampler(2, m_robot_polygons[1], m_room, m_collision_detector);

}

Planner::~Planner()
{}

#define PI 3.14159265

/*	This function is invoked by the GUI and is assumed to update the resulting */
void Planner::run()
{
	m_roadmap_robot1 = new SinglePrm(1,
		m_number_of_nodes,
		12,
		m_collision_detector, 
		robot_1_sampler,
		m_start_confs[0],
		m_target_confs[0]);

	m_roadmap_robot2 = new SinglePrm(2,
		m_number_of_nodes,
		12,
		m_collision_detector, 
		robot_2_sampler,
		m_start_confs[1],
		m_target_confs[1]);

	m_oracle = new Oracle(m_roadmap_robot1, m_roadmap_robot2, m_collision_detector);

	m_roadmap_robot1->generate_roadmap();
	m_roadmap_robot2->generate_roadmap();

	double start_config_array[] = {CGAL::to_double(m_start_confs[0].x()),
		CGAL::to_double(m_start_confs[0].y()),
		CGAL::to_double(m_start_confs[1].x()),
		CGAL::to_double(m_start_confs[1].y())};

	Point_d start_configuration_point(4, start_config_array, start_config_array + 4);

	double target_config_array[] = {CGAL::to_double(m_target_confs[0].x()),
		CGAL::to_double(m_target_confs[0].y()),
		CGAL::to_double(m_target_confs[1].x()),
		CGAL::to_double(m_target_confs[1].y())};

	Point_d target_configuration_point(4, target_config_array, target_config_array + 4);

	m_t1 = new ExpendedTree(start_configuration_point, m_oracle, m_multi_robots_sampler);
	m_t2 = new ExpendedTree(target_configuration_point, m_oracle, m_multi_robots_sampler);

	int i = 1;

	while (true)
	{
		m_t1->expand((int)pow(2.0,i));
		m_t2->expand((int)pow(2.0,i));

		connect_trees(i);

		if (m_path.size() != 0)
		{
			break;
		}

		i++;
	}
	//	run this method when you finish to produce the path
	//	IMPORTANT: the result should be put in m_path
	transform_path_for_gui();
}

void Planner::connect_trees( int number_of_iterations )
{
	for (int i = 0; i < number_of_iterations; ++i)
	{
		Point_d q_rand = m_multi_robots_sampler->generate_sample_point_d();

		vector<Point_d> t_1_k_nearest_neighbors;
		m_t1->_kd_tree->k_nearest_neighbors(q_rand,
			1,
			std::back_inserter(t_1_k_nearest_neighbors));
		Point_d q_1_near = t_1_k_nearest_neighbors[0];
		int q_1_near_index = m_t1->get_point_index(q_1_near);

		vector<Point_d> t_2_k_nearest_neighbors;
		m_t2->_kd_tree->k_nearest_neighbors(q_rand,
			1,
			std::back_inserter(t_2_k_nearest_neighbors));
		Point_d q_2_near = t_2_k_nearest_neighbors[0];
		int q_2_near_index = m_t2->get_point_index(q_2_near);

		AStar a_star(q_1_near, q_2_near, m_oracle, m_collision_detector);
		vector<ConfSet> path = a_star.get_path();

		if (path.size() != 0)
		{
			std::list<int> temp_path;

			m_t1->_tree->find_path(0, q_1_near_index,temp_path);

			for (std::list<int>::iterator it = temp_path.begin();
				it != temp_path.end();
				++it)
			{
				m_path.push_back(a_star.get_conf_set(m_t1->_all_tree_points[*it]));
			}

			for (vector<ConfSet>::iterator it = path.begin();
				it != path.end();
				++it)
			{
				m_path.push_back(*it);
			}

			temp_path.clear();

			m_t2->_tree->find_path(q_2_near_index, 0,temp_path);

			for (std::list<int>::iterator it = temp_path.begin();
				it != temp_path.end();
				++it)
			{
				m_path.push_back(a_star.get_conf_set(m_t2->_all_tree_points[*it]));
			}
		}
	}
}

Point_2	Planner::transformToCGAL(QPointF qp)
{
	// x and y coordinates of the point
	double x = qp.x();
	double y = qp.y();

	Point_2 cpoint(x,y);
	return cpoint;
}
QPointF	Planner::transformToQT(Conf cp)
{
	return QPointF(CGAL::to_double(cp.x()) , CGAL::to_double(cp.y()) );
}

QVector<QPointF> Planner::transformToQT(ConfSet cs)
{
	QVector<QPointF> result;

	foreach(Conf c, cs)
	{
		result.push_back(transformToQT(c));
	}
	return result;
}

QVector<QVector<QPointF> > Planner::transformToQT(list<vector<Conf> > path)
{
	QVector<QVector<QPointF> > result;

	foreach(ConfSet cs, path)
	{
		result.push_back(transformToQT(cs));
	}
	return result;
}

void Planner::transform_path_for_gui()
{
	vector<vector<vector<QConf> > > result_vectors(m_path.size());

	for (int i = 0; i < m_path.size(); i++)
	{
		for (int r = 0; r < m_robot_num; r++)
		{
			vector<QConf> path_for_r;

			QPointF vec;
			double angle = 0;
			if (i != 0)
			{
				QPointF s = transformToQT(m_path[i-1][r]);
				QPointF t = transformToQT(m_path[i][r]);
				vec = t - s;
			}
			else
				vec = transformToQT(m_path[i][r]);

			QConf qconf(vec,angle);
			path_for_r.push_back(qconf);

			result_vectors[i].push_back(path_for_r);
		}
	}

	/*	If "result" is empty, the GUI assumes that a solution haven't been found */
	m_scene->setPath(result_vectors);	
}

void Planner::extract_scenario_from_gui()
{
	Point_2 ptl = transformToCGAL(m_scene->getRoomTopLeft());
	Point_2 pbr = transformToCGAL(m_scene->getRoomBottomRight());
	Point_2 pbl(ptl.x(), pbr.y());
	Point_2 ptr(pbr.x(), ptl.y());
	m_room = Room(ptl,pbr);

	QVector<QPolygonF> obstacles = m_scene->getObstacles();

	vector<QPolygonF> robots = m_scene->getRobotPolygons();
	foreach(QPolygonF qp, robots)
	{
		Polygon_2 cp;
		for (int j=0; j < qp.size(); j++)
		{
			cp.push_back(transformToCGAL(qp[j]));
		}

		m_robot_polygons.push_back(cp);
	}

	foreach(QPolygonF qp, obstacles)
	{
		Polygon_2 cp;
		for (int j=0; j < qp.size(); j++)
		{
			cp.push_back(transformToCGAL(qp[j]));
		}

		m_obstacles.push_back(cp);
	}

	Polygon_2 wall_left;
	wall_left.push_back(pbl);
	wall_left.push_back(ptl);

	Polygon_2 wall_top;
	wall_top.push_back(ptl);
	wall_top.push_back(ptr);

	Polygon_2 wall_right;
	wall_right.push_back(ptr);
	wall_right.push_back(pbr);

	Polygon_2 wall_bot;
	wall_bot.push_back(pbl);
	wall_bot.push_back(pbr);

	m_obstacles.push_back(wall_left);
	m_obstacles.push_back(wall_top);
	m_obstacles.push_back(wall_right);
	m_obstacles.push_back(wall_bot);

	QVector<QVector<QPointF> > startPositions = m_scene->getStartPositions();
	QVector<QVector<QPointF> > targetPositions = m_scene->getTargetPositions();

	m_robot_num = targetPositions.size();

	for (int g = 0; g < m_robot_num; g++)
	{
		m_start_confs.push_back(transformToCGAL(startPositions[g][0]));
		m_target_confs.push_back(transformToCGAL(targetPositions[g][0]));
	}
}
